#include <future>
#include "fsm_booting.h"
#include "axisgroup.h"
#include "errcode.h"
#include "log/zlog.hh"
#include "axis/axis.h"
#include "motion/modbuscrc.h"
#include "axisgroup_set.h"

using axisgroup::AxisGroup;
using axisgroup::FsmBooting;

#define ROB_DBG(fmt, ...) zlog()->debug("[AxisGroup{}] Booting:" fmt, grp_->get_group_id(), ##__VA_ARGS__)
#define ROB_ERR(fmt, ...) zlog()->error("[AxisGroup{}] Booting:" fmt, grp_->get_group_id(), ##__VA_ARGS__)

FsmBooting::FsmBooting(AxisGroup* grp) : grp_(grp) { reset(); }
void FsmBooting::reset()
{
    is_timeout_ = false;
    watchdog_cnt_ = WATCHDOG_TIMEOUT_MS;
    boot_stat_ = 1;
}
void FsmBooting::update(bool is_first)
{
    if (is_first)
    {
        reset();
    }

    grp_->cmd_.enable = false;
    for (auto& axis : grp_->joints) { axis->enable(false); }

    if (grp_->is_sim())
    {
        // 仿真默认booting自动完成,跳过所有boot步骤
        boot_stat_ = 5;
    }

    if (check_watchdog())
    {
        return;
    }
    bool is_ready = true;
    switch (boot_stat_)
    {
    case 1: {
        if (check_all_axis_ready())
        {
            ROB_DBG("Find all joints");
            if (grp_->RtRobot()->query_robot_serial_num())
            {
                ROB_ERR("Cannot read robot's serial num!");
                grp_->change_fsm_stat(AxisGroup::FsmState::ErrorStop);  // 读不到序列号
                break;
            }
            if (grp_->RtRobot()->get_robot_serial_num() != grp_->serial_num_ && grp_->RtRobot()->get_robot_serial_num() != 0)
            {
                grp_->boot_init(grp_->RtRobot()->get_robot_serial_num(), true);  // 重新加载配置与轴
                break;
            }
            boot_stat_ = 2;
        }
    }
    break;
    case 2: {
        int res = sync_boot_data();
        if (0 == res)
        {
            ROB_DBG("Data sync done");
            boot_stat_ = 3;
        }
        else if (-2 == res)
        {
            // 机型有更新,跳过剩下的步骤
            boot_stat_ = 5;
        }
        break;
    }

    case 3:
        if (0 == sync_dh_data())
        {
            ROB_DBG("Dh param sync done");
            boot_stat_ = 4;
        }
        break;
    case 4:
        if (0 == sync_dyna_data())
        {
            ROB_DBG("Dyna param sync done");
            boot_stat_ = 5;
        }
        break;
    default:
        break;
    }
    if (boot_stat_ == 5)
    {
        ROB_DBG("Boot done");
        grp_->change_fsm_stat(AxisGroup::FsmState::Disabled);
        grp_->reset_joint_planner();
    }
}

bool FsmBooting::check_all_axis_ready()
{
    for (auto& axis : grp_->joints)
    {
        if (!axis->is_alive())
        {
            return false;
        }
    }
    // TODO: 检查轴id是否连续

    return true;
}
void FsmBooting::timeout_action()
{
    if (!is_timeout_)
    {
        is_timeout_ = true;
        if (!check_all_axis_ready())
        {
            std::string jids_str = "[ ";
            for (auto& axis : grp_->joints)
            {
                if (axis->is_alive())
                {
                    jids_str += std::to_string(axis->get_id()) + " ";
                }
            }
            jids_str += "]";
            ROB_ERR("Boot timeout. Only axis {} ready. Action:{}", jids_str, grp_->config_.action_after_boot_timeout);
        }
        else
        {
            ROB_ERR("Boot timeout. Sync over time. Action:{}", grp_->config_.action_after_boot_timeout);
        }
    }

    // boot 所有轴超时，尝试读取机器人序列号，因为可能boot失败是由于连接的为6轴机器人
    if (boot_stat_ == 1)
    {
        if (grp_->RtRobot()->query_robot_serial_num())
        {
            ROB_ERR("Cannot read robot's serial num!");
        }
        else if (grp_->RtRobot()->get_robot_serial_num() != grp_->serial_num_)
        {
            grp_->change_fsm_stat(AxisGroup::FsmState::Disabled);  // 机型不符，等待后续重新加载再次boot
            return;
        }
    }

    if (grp_->config_.action_after_boot_timeout == ERR_AFTER_BOOT_TIMEOUT)
    {
        grp_->change_to_errstat(RobEvCode::BOOT_SYNC_DATA_TIMEOUT, false);
    }
    else
    {
        grp_->shutdown();
    }
}
int FsmBooting::check_watchdog()
{
    if (watchdog_cnt_ > 0)
    {
        watchdog_cnt_ -= grp_->config_.cyclictime_sec * 1000;
        return 0;
    }

    timeout_action();
    return -1;
}
int FsmBooting::sync_boot_data()
{
#if 0
    RobJointData<int> boot_data_sync_sta_(0xffffffff);

    for (size_t i = 0; i < grp_->joint_num(); i++)
    {
        auto axis = grp_->joints[i];
        axis->sync_collision_lv(grp_->config_.safety.clsn_sensitivity_lv[i]);

        axis->async_set_min_pos(grp_->config_.safety.min_jpos[i]);
        axis->async_set_max_pos(grp_->config_.safety.max_jpos[i]);
        axis->async_set_errpos(grp_->config_.safety.jpos_eps[i]);
        axis->async_set_torq_unit(2);
        axis->async_get_collision_base(&grp_->state_.collision_base[i]);
        axis->async_get_collision_base(&grp_->state_.collision_torque[i]);
        axis->async_get_first_power_date(&grp_->state_.first_power_date[i]);
        boot_data_sync_sta_[i] = axis->get_config_sync_sta();
    }

    bool all_sync_done = true;
    for (size_t i = 0; i < grp_->joint_num(); i++)
    {
        if (boot_data_sync_sta_[i] != 0)
        {
            all_sync_done = false;
            break;
        }
    }

    return all_sync_done ? 0 : -1;
#else
    return 0;
#endif
}

bool FsmBooting::check_robot_param_CRC(int type)
{
    if (type == 0)
    {
        auto dh = grp_->RtRobot()->get_dh();
        if (dh.version > 3)
        {
            return false;
        }
        uint16_t dataCrc = 0;
        uint32_t check_para[MAX_AXIS * 8] = {};
        size_t joint_num = grp_->joint_num();
        for (size_t i = 0; i < joint_num; i++) { check_para[i] = dh.version; }
        for (size_t i = 0; i < joint_num; i++)
        {
            check_para[joint_num + 4 * i + 0] = dh.joint[i].a;
            check_para[joint_num + 4 * i + 1] = dh.joint[i].alpha;
            check_para[joint_num + 4 * i + 2] = dh.joint[i].d;
            check_para[joint_num + 4 * i + 3] = dh.joint[i].theta;
            if (dh.version > 1)
            {
                check_para[joint_num + 4 * i + 4] = dh.joint[i].base_offset_1;
                if (dh.version > 2)
                {
                    check_para[joint_num + 4 * i + 5] = dh.joint[i].base_offset_2;
                }
            }
        }
        dataCrc = CheckCRC(check_para, (4 + dh.version) * joint_num);
        ROB_DBG("calcCrc = {}, obtained crc = {}\n", dataCrc, dh.crc);
        if (dataCrc == dh.crc)
            return true;
        else
        {
            for (size_t i = 0; i < (4 + dh.version) * joint_num; i++) { ROB_DBG("DH Para {} = {}", i, check_para[i]); }
        }
    }
    else
    {
        // UINT16 dataCrc = CheckCRC((uint32_t*)robotParam.dynParamData, 11 * robot_joint_num);
        // for (int i = 0; i < 11 * robot_joint_num; i++) { rtapi_print("dynParamData[%d] = %d\n", i, robotParam.dynParamData[i]); }
        // rtapi_print("calcCrc = %x, obtained crc = %x\n", dataCrc, robotParam.dynCRC);
        // if (dataCrc == robotParam.dynCRC)
        //     return 0;
    }
    return false;
}

int FsmBooting::sync_dh_data()
{
    // 该接口操作的rtrobot的这块数据，仅motion线程使用，不加锁
    grp_->RtRobot()->query_dh_param();  // 阻塞接口
    auto rt_dh = grp_->RtRobot()->get_dh();
    if (rt_dh.state == rtdev::RtRobot::DHDYNReadStatus::DHYN_READ_OVER)
    {
        if (check_robot_param_CRC(0))
        {
            rt_dh.state = rtdev::RtRobot::DHDYNReadStatus::DHYN_IN_READY;
            RobotDH dh;
            for (size_t i = 0; i < grp_->joint_num(); i++)
            {
                dh.a[i] = rt_dh.joint[i].a / 100000.0;
                dh.d[i] = rt_dh.joint[i].d / 100000.0;
                dh.joint_homeoff[i] = rt_dh.joint[i].theta / 100000.0;
                dh.alpha[i] = rt_dh.joint[i].alpha / 100000.0;
            }
            grp_->set_intf_->set_axisgroup_dh(dh, true);
        }
    }
    // 不管DH读取是否成功，都应跳转至下一状态
    return 0;
}
int FsmBooting::sync_dyna_data()
{
    grp_->RtRobot()->query_dyn_param();
    // TODO:
    return 0;
}
